/*
 * motor_control_core.h
 *
 *  Created on: 2025 Feb 18
 *      Author: SFLY
 */

#ifndef XTFOCLIB_TASKS_SENSOR_MOTOR_CONTROL_CORE_H_
#define XTFOCLIB_TASKS_SENSOR_MOTOR_CONTROL_CORE_H_

#include <hal_can_communicator.h>
#include "bsp_board_config.h"
#include "hal_current_sensor.h"
#include "hal_single_abs_encoder.h"
#include "hal_motor_pwm_controller.h"
#include "hal_motor_protector.h"
#include "hal_uart_communicator.h"
#include "hal_fileio.h"
#include "mod_pid.h"
#include "mod_filter.h"
#include "mod_math.h"
#include "mod_pid.h"
#include "mod_coordinate_transform.h"
//std lib
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

typedef enum {
    Open_Controller                         = 0,    //电流模式
    Close_Pos_Spd_Torque_Current_Controller = 1,    //运控模式
    Close_Pos_Spd_Current_Controller        = 2,    //位置-速度-电流模式
} xt_task_motor_ControllerType;//电机控制模式

typedef enum{
    COMM_MIT = 0, //MIT协议
    COMM_XT = 1,  //自定义协议
}xt_task_motor_CommunicationType;//通信协议

typedef struct{
    uint32_t cur_time;             //当前的时间
    uint32_t last_time;            //上一次的时间
    float deltaTimes;              //秒
    float Motor_End_Theta_mech;   //电机输入减速器前的机械转角
    float Motor_End_Theta_Filtermech;   //电机输入减速器前的机械转角
    float Motor_End_Theta_elec;   //电机输入减速器前的电角速度
    float Motor_End_Theta_mech_prev;
    float Reducer_End_Theta_mech;//电机减速器之后的机械角度(如果没有减速器，该值等于电机本体的机械角度Motor_ThetaMech)该值需要返回
    float Reducer_End_Theta_Filtermech;
    float Reducer_End_Theta_mech_prev;
    float Reducer_End_RoundCnt;//减速器输出端圈数
    float Reducer_End_RoundCnt_prev;
    float Motor_End_mech_Vel_Obser;//观测到的电机本体角速度(rad/s)
    float Motor_End_mech_Vel_FilterObser;//观测到的电机本体角速度(rad/s)
    float Motor_End_mech_Vel_RPM_Obser;//观测到的电机本体角速度(r/min)
    float Motor_End_elec_Vel_Obser;//观测到的电机本体电角速度(rad/s)
    float Reducer_End_mech_Vel_RPM_Obser;//电机减速器之后的机械角速度(r/min)，该值需要返回
    float Reducer_End_mech_Vel_Obser;//电机减速器之后的机械角速度(rad/s)，该值需要返回
    float Reducer_End_mech_Vel_FilterObser;//低通滤波器电机减速器之后的机械角速度(rad/s)，该值需要返回
    float Reducer_End_Torque;//电机减速器之后的力矩
    uint8_t isFirstUpdateFlag; //首次进行测量
    uint32_t ErrorFlag;        //编码器错误标志
    stc_1stLpf_t            velLpf;//电机机械角速度低通滤波器
    EKF_Estimator           PosVelEkf;//电机角度和速度卡尔曼滤波器
    xt_hal_motor_end_single_abs_encoder_t me_encoder;//电机本体编码器
    xt_hal_reducer_end_single_abs_encoder_t re_encoder;//用于减速器端的编码器
}xt_task_motor_DoubleEncoder;//电机编码器结构，现在是一个差速双编码器结构


typedef struct{
    uint32_t Motor_CanID;                                 //电机CANID
    float Motor_StdElecAngle;                             //电机归一化的电角度，范围（0-1）
    uint32_t OneMsCnt;                                      //1ms计数器
    xt_task_motor_DoubleEncoder Motor_Encoder;            //电机编码器参数
    xt_hal_current_sensor_t Motor_Current_Sensor;         //电机电流观测器
    xt_hal_motor_pwm_controller_t Motor_PWM_Controller;   //电机PWM控制器
    xt_hal_Motor_Protector_t Motor_Protector;             //电机保护控制器
    stc_abc_t Motor_IuvwObser;                            //观测到的电机三相电流
    stc_ab_t  Motor_IalphaBetaObser;                      //观测到的电机alpha/beta轴电流
    stc_dq_t  Motor_IdqObser;                             //分解后的电机dq轴电流
    stc_dq_t  Motor_IdqRef;                               //期望的电机dq轴电流
    stc_ab_t  Motor_IalphaBetaRef;                        //期望的电机alpha/beta轴电流
    xt_task_motor_ControllerType    Motor_ControlType;    //电机控制器类型
    uint8_t Motor_Status;                                 //电机当前状态标志位
    uint8_t Motor_Status_Change;                          //电机状态改变标志位
}xt_task_motor_RunstatusObser;//电机运行状态观测器



typedef struct
{
    uint32_t cur_time;      //当前时间
    uint32_t last_time;     //前次时间
    uint32_t SoftStartTimestep;//软启动时间步
    float target_mech_theta;//电机本体经过减速器后输出的目标转角（如果没有减速器，就是电机本体输出轴的）
    float target_mech_aftermotor_theta;//根据减速比计算出来的减速器端的角度
    float target_mech_rpm;  //电机本体经过减速器后输出的目标机械转速（r/min）
    float target_mech_rpms;  //电机本体经过减速器后输出的目标机械转速（r/ms）
    float target_mech_wr;   //电机本体经过减速器后输出的目标机械角速度（rad/s）可以和机械转速之间互相转换
    float target_elec_wr;   //电机本体输出的目标电角速度（rad/s）可以和机械转速之间互相转换
    float target_torque;    //电机本体输出轴的目标转矩N.m（可以用于转矩前馈、也可以简单用于电流环期望Iq的计算）
    float target_iq;        //电机本体的目标q轴电流（可以和力矩结合）
    float target_id;        //电机本体的目标d轴电流（用于零点校准）
    stc_mit_pid_t pos_torque_pid;   //运控模式下的PID参数
    stc_inc_pid_t current_q_pid;    //电流环q轴PID参数
    stc_inc_pid_t current_d_pid;    //电流环d轴PID参数
    stc_pos_pid_t speed_pid;      //速度环PID参数
    stc_pos_pid_t pose_pid;       //位置环PID参数
    float LimitSpd_Max;     //位置模式 速度限制(0-max)
    float LimitCur_Max;     //位置、速度模式电流限制(0-max)
}xt_task_motor_Controller;//电机控制器

typedef struct{
    uint32_t Motor_CanID;
    uint32_t Master_CanID;
    xt_hal_uart_communicator_t uart_commuicator;
    xt_hal_can_communicator_t  can_commuicator;
}xt_task_motor_Communicator;//通信控制器

extern xt_task_motor_RunstatusObser     g_stc_MotorRunStatusObser;//电机运行状态器
extern xt_task_motor_Controller         g_stc_MotorController;//电机控制器
extern xt_task_motor_Communicator       g_stc_MotorCommunicator;//电机通信控制器


void Task_Board_Init(void);
void Task_Motor_RunStatusObser_Init(xt_task_motor_RunstatusObser *pstc);
void Task_Motor_Controller_Init(xt_task_motor_Controller *pstc);
uint8_t Task_Motor_Encoder_Init(xt_task_motor_DoubleEncoder *pstc);
void Task_Motor_Communicator_Init(xt_task_motor_Communicator *pstc);
void Task_Encoder_Calibration(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller,xt_task_motor_Communicator *motorcommunitor);
void Task_Motor_OpenRun(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller);
void Task_Encoder_DirCalibration(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller);
void Task_Encoder_ZeroCalibration(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller);
void Task_Motor_FastStop(xt_task_motor_RunstatusObser *motorstatus, xt_task_motor_Controller *controller);
void Task_Time1msLoopEvent(xt_task_motor_RunstatusObser *motorstatus, xt_task_motor_Controller *controller);
void Task_MainLoopEvent(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller,xt_task_motor_Communicator *motorcommunitor);
void Task_Motor_Encoder_Update(xt_task_motor_DoubleEncoder *motorencoder);
void Task_Motor_Elec_Update(xt_task_motor_RunstatusObser *motorobser);
void Task_Encoder_SavedCalibrationResult(xt_task_motor_RunstatusObser *motorobser,xt_task_motor_Communicator *motorcommunitor);
void Task_Motor_PosSpdLoopCurrentLoopRun(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller);
#endif /* XTFOCLIB_TASKS_SENSOR_MOTOR_CONTROL_CORE_H_ */
